#include "ros/ros.h"
#include "geometry_msgs/Pose.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "biped_robin_msgs/PickupObjectService.h"
#include "biped_robin_msgs/PlaceObjectService.h"
#include "std_srvs/Empty.h"
#include "std_msgs/String.h"
#include <string>

ros::ServiceClient pickup_client;
ros::ServiceClient place_client;
ros::ServiceClient start_detecting_client;
ros::ServiceClient stop_detecting_client;
ros::Publisher chatter_pub;
static tf::TransformListener* pListener = NULL;
static float PI = 3.14159265359;

//Becher aufnehmen
bool pickupCylinder(){
	biped_robin_msgs::PickupObjectService srv;
	srv.request.object.reference_frame_id = "cylinder_center";	
	pickup_client.call(srv);
	return true;
};

//Becher entsorgen
bool placeCylinder(){
	biped_robin_msgs::PickupObjectService srv;
	srv.request.object.reference_frame_id = "bin_frame";
	place_client.call(srv);
	return true;	
};


//Calbacks
bool pickupCylinderCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
	ROS_INFO("pickup_cylinder_srv called");	
	return pickupCylinder();
}

bool placeCylinderCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
	ROS_INFO("place_cylinder_srv called");		
	return placeCylinder();
}



int main(int argc, char **argv)
{
  /* init ROS */    
  ros::init(argc, argv, "biped_localizer");
	pListener = new(tf::TransformListener);
	ros::NodeHandle n;

	//service servers	
	ros::ServiceServer pickup_cylinder_srv = n.advertiseService("pickup_cylinder_srv" , pickupCylinderCallback);
	ros::ServiceServer place_cylinder_srv = n.advertiseService("place_cylinder_srv" , placeCylinderCallback);

	//service clients
	pickup_client = n.serviceClient<biped_robin_msgs::PickupObjectService>("pickup_srv");
	place_client = n.serviceClient<biped_robin_msgs::PickupObjectService>("place_srv");
	start_detecting_client = n.serviceClient<std_srvs::Empty>("start_detecting_srv");
	stop_detecting_client = n.serviceClient<std_srvs::Empty>("stop_detecting_srv");

	//publisher	
	chatter_pub = n.advertise<std_msgs::String>("/speech", 1000);

	tf::StampedTransform odom_cylinder_transform;
	std_srvs::Empty srv;
	ros::Rate loop_rate(1);	
	while(ros::ok){	
		
		bool move = false;
		start_detecting_client.call(srv);
		ros::Time now = ros::Time::now();
		try {
			//auf erkennung des bechers warten
			pListener->waitForTransform("/odom", "/cylinder_center", now, ros::Duration(2.0));
    	pListener->lookupTransform("/odom", "/cylinder_center", now, odom_cylinder_transform);				
			ROS_INFO("CUP FOUND");
			stop_detecting_client.call(srv);
			std_msgs::String msg;
			msg.data = "I found a cup";
			chatter_pub.publish(msg);
			pickupCylinder();
			msg.data = "I will throw away the cup for you";
			chatter_pub.publish(msg);
			placeCylinder();	
			msg.data = "Feed me with another cup";
			chatter_pub.publish(msg);	
		} catch (tf::TransformException ex) {
			ROS_INFO("NO CUP FOUND");
		}	
		loop_rate.sleep();		
		ros::spinOnce();
	}
  return 0;
}
